#ifndef CSTANLEY_H
#define CSTANLEY_H

#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <cmath>

#include "matplotlibcpp.h"

using namespace std;
namespace plt = matplotlibcpp;

typedef struct {
    unsigned long idx;
    double latError;
} minLatError_idx_t;

class CStanley
{
public:
    vector<double> XRef;
    vector<double> YRef;
    vector<vector<double>> posRef;

    // 有关参数
    double k; // 增益参数 过大会导致实际的轨迹发散
    double Kp; // 速度P控制系数
    double dt; // 采样间隔,单位：s
    double L; // 车辆轴距，单位：m
    double targetSpeed; // 目标速度，单位：m/s

    vector<double> initStates; // X, Y, Phi, v
    vector<double> states;
    vector<vector<double>> statesActual;

    unsigned long idxTarget;

    // 保存实际的位置点
    vector<double> XActual;
    vector<double> YActual;

public:
    CStanley();
    ~CStanley();

    inline double normFunc(double posRefX, double posRefY, double posX, double posY);

    //读取路径点
    vector<vector<double>> readPosPoints(vector<double> &XRef, vector<double> &YRef);

    //在参考路径点上，寻找距离当前点最近的距离，及所对应的索引
    minLatError_idx_t findMinLatError_idx(vector<double> states,
                                          vector<vector<double>> posRef);

    double stanleyControl(unsigned long idxTarget, vector<double> states,
                          double latError, vector<vector<double>> posRef, double k);

    vector<double> updateStates(double a, vector<double> states,
                                double delta, double dt, double L);

    void plotRes(vector<double> XRef, vector<double> YRef,
                 vector<double> XActual, vector<double> YActual);
};

#endif // CSTANLEY_H
